﻿#include "xict_ui_core/canmanage.h"
#include "xict_ui_common/utils/config.h"
#include <QProcess>
#include <cstdlib>
#include <iostream>
#include <signal.h>
#include <unistd.h>
namespace xict_ui_core {
    CanManager::CanManager(QObject* parent) : QObject(parent) {
        init();
        process_wika = new QProcess();
        setmode_status(1);
    }

    CanManager::~CanManager() {
        m_udpSender->close();
        m_udpSender->deleteLater();
        if (m_can_thread->isRunning()) {
            m_can_thread->quit();
            m_can_thread->wait();
        }
    }

    void CanManager::runShellScript() {
        process_wika->setWorkingDirectory(xict_ui_common::Config::GetInstance()
                                              .GetYaml("ui", "wika_app_path")
                                              .c_str());
        // 启动 LoaderUI 脚本
        process_wika->start("./LoaderUI");
        if (process_wika->waitForStarted()) {
            // qDebug() << "Started LoaderUI with PID:"
            //  << process_wika->processId();
        }
    }

    void CanManager::stopShellScript() {
        // 终止进程
        process_wika->kill();
    }

    void CanManager::shutdownSystem() {
        // 使用shutdown命令进行关机，`poweroff`也可以替代`shutdown now`
        QProcess process;
        QStringList args;
        args << "now";   // 立即关机
        process.start("shutdown", args);

        if (!process.waitForFinished()) {
            // qWarning() << "Failed to shutdown:" << process.errorString();
        } else {
            // qDebug() << "Shutdown command executed.";
        }
    }

    void CanManager::init() {
        m_ip = QString::fromStdString(
            xict_ui_common::Config::GetInstance().GetYaml("ui",
                                                          "vehicle_can_ip"));
        m_port = std::stoi(xict_ui_common::Config::GetInstance().GetYaml(
            "ui", "vehicle_can_port"));

        m_cockpit_ip = QString::fromStdString(
            xict_ui_common::Config::GetInstance().GetYaml("ui",
                                                          "cockpit_can_ip"));
        m_cockpit_port =
            std::stoi(xict_ui_common::Config::GetInstance().GetYaml(
                "ui", "cockpit_can_port"));

        m_cockpit_udpSender = new QUdpSocket(this);
        m_udpSender         = new QUdpSocket(this);
        m_linePub = new eCAL::protobuf::CPublisher<xcmg_proto::lane_line>(
            xict_ui_common::Config::GetInstance().GetYaml("ui", "lane_line"));

        m_can_thread = new QThread(this);
        m_can_sdk    = new xict_ui_driver::CanSdk();
        m_can_sdk->moveToThread(m_can_thread);
        connect(m_can_sdk, &xict_ui_driver::CanSdk::signalGraderCan, this,
                &CanManager::update_grader_can);
        connect(m_can_sdk, &xict_ui_driver::CanSdk::signalLoaderCan, this,
                &CanManager::update_loader_can);
        connect(m_can_sdk, &xict_ui_driver::CanSdk::signalScraperCan, this,
                &CanManager::update_scraper_can);
        setcamera_changed(1);
    }

    void CanManager::start() {
        m_can_thread->start();
    }

    void CanManager::enableVehicleType(int vehicle_type) {
        m_can_sdk->EnableSdk(vehicle_type);
    }

    void CanManager::disenableVehicleType() {
        m_can_sdk->DisableSdk();
    }
    void
    CanManager::update_scraper_can(const xcmg_proto::Scraper_CAN& can_msg) {
        setcamera_changed(can_msg.camera_changed());
        // qDebug() << "can_msg.camera_changed" << can_msg.camera_changed();
        setengine_speed(can_msg.engine_speed());
        setcoolant_temperature(can_msg.coolant_temperature());
        setoil_pressure(can_msg.oil_pressure());
        setoil_temperature(can_msg.oil_temperature());
        setengine_operating_time(can_msg.engine_operating_time());
        setengine_total_fuel_consumption(
            can_msg.engine_total_fuel_consumption());
        setarm_angle(can_msg.arm_angle());
        setfuel_level(can_msg.fuel_level());
        setturn_angle(can_msg.turn_angle());
        setbucket_angle(can_msg.bucket_angle());
        setarm_height(can_msg.arm_height());
        setvehicle_angle_x(can_msg.vehicle_angle_x());
        setvehicle_angle_y(can_msg.vehicle_angle_y());
        setspeed(can_msg.speed());
        setforward_back_signal(can_msg.forward_back_signal());
        setparking_signal(can_msg.parking_signal());
        setgear(can_msg.gear());
        setcontrol_change(can_msg.control_change());
        setlow_power_ready_status(can_msg.low_power_ready_status());
        setlow_power_status(can_msg.low_power_status());
        setcontrol_mode(can_msg.control_mode());

        seterror_communication_vehicle_controller_and_console_controller(
            can_msg
                .error_communication_vehicle_controller_and_console_controller());
        seterror_anticollision_limited_action(
            can_msg.error_anticollision_limited_action());
        seterror_communication_imu(can_msg.error_communication_imu());
        seterror_communication_biaxial_inclination_sensor(
            can_msg.error_communication_biaxial_inclination_sensor());
        seterror_communication_vehicle_pc_and_vehicle_controller(
            can_msg.error_communication_vehicle_pc_and_vehicle_controller());
        seterror_communication_left_handle(
            can_msg.error_communication_left_handle());
        seterror_communication_right_handle(
            can_msg.error_communication_right_handle());
        seterror_communication_left_button_borad(
            can_msg.error_communication_left_button_borad());
        seterror_communication_right_front_button_board(
            can_msg.error_communication_right_front_button_board());
        seterror_communication_engine(can_msg.error_communication_engine());
        seterror_communication_right_back_button_board(
            can_msg.error_communication_right_back_button_board());
        seterror_communication_vehicle_controller_and_console_pc(
            can_msg.error_communication_vehicle_controller_and_console_pc());
        seterror_engine_air_filter_clogged(
            can_msg.error_engine_air_filter_clogged());
        seterror_low_hydraulic_oil_level(
            can_msg.error_low_hydraulic_oil_level());
        seterror_low_transmission_oil_level(
            can_msg.error_low_transmission_oil_level());
        seterror_low_fuel_level(can_msg.error_low_fuel_level());
        seterror_high_hydraulic_oil_temperature(
            can_msg.error_high_hydraulic_oil_temperature());
        seterror_connection_left_front_ultrasonic_radar(
            can_msg.error_connection_left_front_ultrasonic_radar());
        seterror_connection_left_back_ultrasonic_radar(
            can_msg.error_connection_left_back_ultrasonic_radar());
        seterror_connection_right_front_ultrasonic_radar(
            can_msg.error_connection_right_front_ultrasonic_radar());
        seterror_connection_right_back_ultrasonic_radar(
            can_msg.error_connection_right_back_ultrasonic_radar());
        seterror_connection_corner_angle_displacement_sensor(
            can_msg.error_connection_corner_angle_displacement_sensor());
        seterror_connection_arm_angle_displacement_sensor(
            can_msg.error_connection_arm_angle_displacement_sensor());
        seterror_connection_bucket_angle_displacement_sensor(
            can_msg.error_connection_bucket_angle_displacement_sensor());
        seterror_speed_sensor(can_msg.error_speed_sensor());
        seterror_communication_front_lidar(
            can_msg.error_communication_front_lidar());
        seterror_communication_back_lidar(
            can_msg.error_communication_back_lidar());
        seterror_solenoid_valve_disconnected_turn_left(
            can_msg.error_solenoid_valve_disconnected_turn_left());
        seterror_solenoid_valve_disconnected_turn_right(
            can_msg.error_solenoid_valve_disconnected_turn_right());
        seterror_solenoid_valve_disconnected_arm_up(
            can_msg.error_solenoid_valve_disconnected_arm_up());
        seterror_solenoid_valve_disconnected_arm_down(
            can_msg.error_solenoid_valve_disconnected_arm_down());
        seterror_solenoid_valve_disconnected_bucket_up(
            can_msg.error_solenoid_valve_disconnected_bucket_up());
        seterror_solenoid_valve_disconnected_bucket_down(
            can_msg.error_solenoid_valve_disconnected_bucket_down());
        seterror_network_latency(can_msg.error_network_latency());
        seterror_low_voltage(can_msg.error_low_voltage());
        seterror_vehicle_lean(can_msg.error_vehicle_lean());
        seterror_solenoid_valve_forward(can_msg.error_solenoid_valve_forward());
        seterror_solenoid_valve_back(can_msg.error_solenoid_valve_back());
        seterror_front_lidar_mirror_contamination(
            can_msg.error_front_lidar_mirror_contamination());
        seterror_back_lidar_mirror_contamination(
            can_msg.error_back_lidar_mirror_contamination());
        seterror_communication_safety_door_a(
            can_msg.error_communication_safety_door_a());
        seterror_communication_safety_door_b(
            can_msg.error_communication_safety_door_b());
        seterror_invasions_safety_door_a(
            can_msg.error_invasions_safety_door_a());
        seterror_invasions_safety_door_b(
            can_msg.error_invasions_safety_door_b());
        setkey_screen_switch_enabled(can_msg.key_screen_switch_enabled());
        setkey_data_load_query_enabled(can_msg.key_data_load_query_enabled());
        setkey_decrement_bucket_enabled(can_msg.key_decrement_bucket_enabled());
        setkey_lattice_limit_release_enabled(
            can_msg.key_lattice_limit_release_enabled());
        setkey_sensor_limit_release_enabled(
            can_msg.key_sensor_limit_release_enabled());
        setkey_door_lock_override_enabled(
            can_msg.key_door_lock_override_enabled());
        setkey_f1_enabled(can_msg.key_f1_enabled());
        setkey_ok_enabled(can_msg.key_ok_enabled());
    }
    void CanManager::update_grader_can(const xcmg_proto::Grader_CAN& can_msg) {
    }
    void CanManager::update_loader_can(const xcmg_proto::Loader_CAN& msg) { }

    void
    CanManager::canToVehicleUdp(const CanManager::can_msgs can_rx_msg_ptr) {
        uint8_t info = can_rx_msg_ptr.dlc;
        QByteArray vSendData;
        vSendData.resize(13);

        if (can_rx_msg_ptr.is_rtr) {
            // info |= 0x40;
        }

        if (can_rx_msg_ptr.is_extended) {
            info |= 0x80;
        }

        vSendData[0] = info;
        xict_common::utils::Uint32_bytes id {};
        id.data      = can_rx_msg_ptr.id;
        vSendData[1] = id.bytes[3];
        vSendData[2] = id.bytes[2];
        vSendData[3] = id.bytes[1];
        vSendData[4] = id.bytes[0];
        for (uint8_t i = 0; i < can_rx_msg_ptr.dlc; i++) {
            vSendData[5 + i] = can_rx_msg_ptr.data[i];
        }

        m_udpSender->writeDatagram(vSendData, QHostAddress(m_ip), m_port);
        vSendData.clear();   /*!<清空内容*/
        vSendData.squeeze(); /*!<释放内存*/
    }

    void CanManager::canToCockpitUdp(const can_msgs can_rx_msg_ptr) {
        uint8_t info = can_rx_msg_ptr.dlc;
        QByteArray vSendData;
        vSendData.resize(13);

        if (can_rx_msg_ptr.is_rtr) {
            // info |= 0x40;
        }

        if (can_rx_msg_ptr.is_extended) {
            info |= 0x80;
        }

        vSendData[0] = info;
        xict_common::utils::Uint32_bytes id {};
        id.data      = can_rx_msg_ptr.id;
        vSendData[1] = id.bytes[3];
        vSendData[2] = id.bytes[2];
        vSendData[3] = id.bytes[1];
        vSendData[4] = id.bytes[0];
        for (uint8_t i = 0; i < can_rx_msg_ptr.dlc; i++) {
            vSendData[5 + i] = can_rx_msg_ptr.data[i];
        }

        m_cockpit_udpSender->writeDatagram(
            vSendData, QHostAddress(m_cockpit_ip), m_cockpit_port);
        vSendData.clear();   /*!<清空内容*/
        vSendData.squeeze(); /*!<释放内存*/
    }
    void CanManager::sendUdpState(int index) {
        can_msgs can;
        can.is_extended = true;
        can.is_rtr      = true;
        can.dlc         = 8;
        //    can.id=0x000001FD; //5505
        can.id = 0x18F6109A;   // 3505
        switch (index) {
        case 1:
            // qDebug() << "1.......";
            {
                can.data[0] = 1 << 0;
                can.data[1] = 0x00;
            }
            break;
        case 2:
            // qDebug() << "2.......";
            {
                can.data[0] = 1 << 2;
                can.data[1] = 0x00;
            }
            break;
        case 3:
            // qDebug() << "3.......";
            {
                can.data[0] = 1 << 4;
                can.data[1] = 0x00;
            }
            break;
        case 4:
            // qDebug() << "4.......";
            {
                can.data[0] = 1 << 6;
                can.data[1] = 0x00;
            }
            break;
        case 5:
            // qDebug() << "5.......";
            {
                can.data[0] = 0x00;
                can.data[1] = 1 << 0;
            }
            break;
        case 6:
            // qDebug() << "6.......";
            {
                can.data[0] = 1 << 5;
                can.data[1] = 1 << 2;
            }
            break;
            // 从这往下均为测试数据
        case 7:
            // qDebug() << "7.......";
            {
                can.data[0] = 0x00;
                can.data[1] = 1 << 4;
            }
            break;
        case 8:
            // qDebug() << "8.......";
            {
                can.data[0] = 0x00;
                can.data[1] = 1 << 6;
            }
            break;
        default: {
            // qDebug() << "none.......";
            can.data[0] = 0x00;
            can.data[1] = 0x00;
        }
        }

        can.data[2] = 0x00;
        can.data[3] = 0x00;
        can.data[4] = 0x00;
        can.data[5] = 0x00;
        can.data[6] = 0x00;
        can.data[7] = 0x00;
        canToVehicleUdp(can);
    }

    void CanManager::sendKeyData(const QVector<int>& pressStates) {
        // qDebug() << "Received Press States:" << pressStates;
        if (pressStates.size() != 8)
            return;
        can_msgs can;
        can.is_extended = true;
        can.is_rtr      = true;
        can.dlc         = 8;
        can.id          = 0x000001BB;
        can.data[0]     = pressStates[0] << 0 | pressStates[1] << 2 |
                      pressStates[2] << 3 | pressStates[3] << 7;
        can.data[1] = pressStates[4] << 0 | pressStates[5] << 1 |
                      pressStates[6] << 2 | pressStates[7] << 3;
        can.data[2] = 0x00;
        can.data[3] = 0x00;
        can.data[4] = 0x00;
        can.data[5] = 0x00;
        can.data[6] = 0x00;
        can.data[7] = 0x00;
        canToVehicleUdp(can);
        can.id = 0x000001BF;
        canToCockpitUdp(can);
    }

    void CanManager::sendLineData(const QVector<int>& line_state) {
        // bool front_wheel_driving_line = 1;
        // bool blade_operation_line     = 2;
        // bool blade_max_operation_line = 3;
        // bool set_zero_height          = 4;
        // bool back_wheel_driving_line = 5;
        if (line_state.size() != 5)
            return;
        xcmg_proto::lane_line lan_line;
        lan_line.set_front_wheel_driving_line(line_state[0]);
        lan_line.set_blade_operation_line(line_state[1]);
        lan_line.set_blade_max_operation_line(line_state[2]);
        lan_line.set_set_zero_height(line_state[3]);
        lan_line.set_back_wheel_driving_line(line_state[4]);
        m_linePub->Send(lan_line);
    }

    void CanManager::sendSetZeroHeight(bool set_zero) {
        //    xcmg_proto::Blade_Sensor blade_sensor;
        //    blade_sensor.set_set_zero_height(set_zero);
        //    qDebug()<<"set zero blade height" <<
        //    blade_sensor.set_zero_height() ; m_bladePub->Send(blade_sensor);
    }

}   // namespace xict_ui_core
